#ifndef BTL_DEMO_FEATUREDEMO_QFEATURETRACKINGDEMOAPPLICATION
#define BTL_DEMO_FEATUREDEMO_QFEATURETRACKINGDEMOAPPLICATION

#include <QApplication>
#include <QTimer>
#include <btl/extra/VideoSource/VideoSourceUEye.hpp>
#include <btl/extra/Gui/QImageView.hpp>
#include <btl/Utility/OpenCVSupport.hpp>
#include <btl/extra/Gui/ImagePainter.hpp>
#include <btl/Image/ColourConversion.hpp>
#include <btl/Features/FeatureDescriptor.hpp>
#include <btl/Features/FeatureMatcher.hpp>

#include <Eigen/Dense>
#include <boost/program_options.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <iostream>
#include <iterator>
#include <functional>
#include <cassert>

class QFeatureTrackingDemoApplication : public QApplication
{

      Q_OBJECT

   public:

      QFeatureTrackingDemoApplication(int& argc, char** argv);
      ~QFeatureTrackingDemoApplication();

   protected Q_SLOTS:

      void update();

   protected:
      static const int PATCH_SIZE = 9;
      static const int MAX_FEATURE_AGE = 3;
      static const int MAX_MATCH_RADIUS = 15;
      static const double MIN_MATCH_SCORE;
      typedef btl::PatchFeatureDescriptor<btl::PixelGrey, PATCH_SIZE, PATCH_SIZE> patch_descriptor_type;

      class DetectedFeature
      {
         public:
            typedef patch_descriptor_type feature_descriptor_type;
            DetectedFeature(Eigen::Vector2f pos, const feature_descriptor_type& descriptor):
               _pos(pos), _descriptor(descriptor) {}
            Eigen::Vector2f position() const { return _pos; }
            const feature_descriptor_type& descriptor() const { return _descriptor; }
         protected:
            Eigen::Vector2f _pos;
            feature_descriptor_type _descriptor;
      };

      class TrackedFeature
      {

         public:

            class IsOlderThan :
               public std::unary_function<bool, const TrackedFeature&>
            {
               public:
                  IsOlderThan(int frameIndex): _frameIndex(frameIndex) {}
                  bool operator()(const TrackedFeature& feature) const
                  {
                     return feature.lastSeenAt() < _frameIndex;
                  }
               private:
                  int _frameIndex;
            };

            typedef patch_descriptor_type feature_descriptor_type;

            TrackedFeature(int frameIndex, Eigen::Vector2f pos, feature_descriptor_type descriptor) :
               _pos(pos), _firstPos(pos), _descriptor(descriptor), _firstSeenAt(frameIndex), _lastSeenAt(frameIndex) {}

            Eigen::Vector2f position() const { return _pos; }

            Eigen::Vector2f firstPosition() const { return _firstPos; }

            const feature_descriptor_type& descriptor() const
            {
               return _descriptor;
            }

            void updatePosition(int frameIndex, const Eigen::Vector2f& pos)
            {
               _pos = pos;
               _lastSeenAt = frameIndex;
            }

            int firstSeenAt() const
            {
               return _firstSeenAt;
            }

            int lastSeenAt() const
            {
               return _lastSeenAt;
            }

         protected:

            Eigen::Vector2f _firstPos;
            Eigen::Vector2f _pos;

            feature_descriptor_type _descriptor;

            int _firstSeenAt;
            int _lastSeenAt;

      };

      struct ScoreNCC
      {
         double operator()(const patch_descriptor_type& a, const patch_descriptor_type& b) const
         { return ncc(a, b); }
      };

      struct FeatureTrackUpdater :
         public std::unary_function < void,
            std::pair <
            std::vector<TrackedFeature>::iterator,
            std::vector<DetectedFeature>::iterator > >
      {
         FeatureTrackUpdater(int frameIndex): _frameIndex(frameIndex) {}

         void operator()(const std::pair <
                         std::vector<TrackedFeature>::iterator,
                         std::vector<DetectedFeature>::const_iterator > & pair) const
         {
            update(*pair.first, *pair.second);
         }

         void update(TrackedFeature& track, const DetectedFeature& observation) const
         {
            track.updatePosition(_frameIndex, observation.position());
         }

         int _frameIndex;
      };

      void trackFeatures(btl::ImageRegionConstGrey frame);
      void dropDeadFeatures();
      void initialiseNewFeatures(btl::ImageRegionConstGrey frame);
      void visualiseFeatures();

      QTimer* _pTimer;

      int _frameIndex;

      btl::extra::VideoSource* _pVideoSource;

      btl::extra::QImageViewRGB* _pView;

      cv::FastFeatureDetector _detector;

      btl::ImageRGB _displayImage;

      btl::extra::ImagePainter<btl::PixelRGB> _painter;

      std::vector<DetectedFeature> _detectedFeatures;

      std::vector<TrackedFeature> _trackedFeatures;

      btl::extra::QImageViewGrey* _pPatchView;

};

#endif //BTL_DEMO_FEATUREDEMO_QFEATURETRACKINGDEMOAPPLICATION
